/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : gb.c
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/2
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/2  Dayuan    initial version
*=============================================================================*/
/* System Includes -----------------------------------------------------------*/
#include <math.h>
#include <cmsis_os.h>
#include "FreeRTOS.h"
#include "task.h"
/* Public Includes -----------------------------------------------------------*/
#include "tim.h"
#include "gpio.h"
#include "usart.h"
#include "std.h"
#include "sensor_if.h"
#include "pc/DataScope_DP.h"
#include "timer_if.h"
#include "encoder_if.h"
#include "algorithm_if.h"
#include "control_if.h"
#include "ros_if.h"
/* Private Includes ----------------------------------------------------------*/
#include "gb.h"
#include "gb_if.h"
/* Global variables ----------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/
static SYS_CMD sys_cmd = {GB_STOP, GB_POSITION};
static FSM_STATUS gb_status_machine = FSM_INITIAL;
int jscope_force_raw;

/* Global functions ----------------------------------------------------------*/
void gb(void *argument)
{
	static uint16_t scope_count = 0;
	static uint8_t Send_Count;
//	static float deisred_angle = 0;
//	static uint8_t control_switch = 0;
	//static float  max_cycle = 0; for test -- cdy 2019.9.11
	//static uint8_t clear;
	
	joint_info.angle_des = 0;
	joint_info.im_offset = 0;
	
//	static uint8_t polling_count = 10;
	
	MOTOR_DISABLE();
	sys_time_init();
	enc_reset_create();
	ros_initial();
	sbt_initial();
	HAL_TIM_PWM_Start(&htim14,TIM_CHANNEL_1);
	filter_create(&joint_info);//initial all 1th filter	
	joint_info.pid_out[0] = 0;
	joint_info.pid_out[1] = 0;
	joint_info.angle_fil.out=0;
	joint_info.inc[1] = 0;
	//MOTOR_ENABLE();
  /* Infinite loop */
	motor_enable();
  while(1)
  {
		parse_enc_data(&sbt_force,&enc_inc,&joint_info);
		//max_cycle = task_cycle(get_sys_time_usec(),clear); //for test
		parse_ros_info(&sys_cmd,&joint_info);
		sbt_get_torque();
		/*
		gravity();
		*/

		/*	error detect	*/
		joint_info.error_code = error_detect(&sbt_force,&enc_inc,&joint_info);
		if (joint_info.error_code){
			gb_status_machine = FSM_ERROR;
		}
		/*	SYSTEM INITIAL STATUS	*/
		if(gb_status_machine == FSM_INITIAL)
		{
			/*	user status begin 0*/
			sys_cmd.main_mode = GB_STOP;
			sys_cmd.sub_mode = GB_POSITION;
			desired_generator(&joint_info,1);
			reset_pid_error(&joint_info);
			if(sbt_force.detect_valid == TRUE)
			{
				gb_status_machine = FSM_CALIBRATION;	
				motor_enable();
			}
			else
			{
				sbt_initial();
				enc_reset_create();
			}
			/*	user status end 0*/
		}
		/*	ENCODER CALIBRATION STATUS	*/
		else if(gb_status_machine == FSM_CALIBRATION)
		{
			/*	user status begin	1*/
			if(enc_inc.valid != TRUE)
				motor_encoder_calibration(-10,&joint_info);
			else
			{
				gb_status_machine = FSM_READY;
				pid_reset(&joint_info);
			}
			/*	user status end 1*/
		}
		/*	SYSTEM RUNNIGN STATUS	*/
		else if (gb_status_machine == FSM_READY)
		{
			/*	user status begin	2*/
			/*	control algorithm	*/
			if(sys_cmd.main_mode == GB_STOP)
			{
				motor_disable();
			}
			else if(sys_cmd.main_mode == GB_START)
			{
				motor_enable();
				if(sys_cmd.sub_mode == GB_POSITION)
				{
					#ifdef SPEED_CONTROL
					if(!control_switch)
					{
						joint_info.speed_des[CURRENT] = deisred_angle;
						pid_speed_control(&joint_info);
					}
					else
					{
						joint_info.angle_des = deisred_angle;
						pid_position_control(&joint_info);
					}
					#else
					desired_generator(&joint_info,0);
					smc_controller(&joint_info);
					//pid_position_control(&joint_info);
					#endif
					
					joint_info.pwm = motor_control(joint_info.f_out);
				}
				else if(sys_cmd.sub_mode == GB_IMPADENCE)
				{
					#ifdef PID_DEBUG
					//desired_generator(&joint_info,0);
					#endif
					impadence_control(&joint_info);
					pid_control(&joint_info);
					//smc_controller(&joint_info);
					joint_info.pwm = motor_control(joint_info.f_out);
				}

			}
		}
			/*	user status end 2*/
		/*	SYSTEM ERROR HANDLE	*/
		else if(gb_status_machine == FSM_ERROR)
		{
			/*	user status begin	3*/
			motor_disable();
			if(sys_cmd.main_mode == GB_RESTART)
				gb_status_machine = FSM_INITIAL;
			/*	user status end 3*/
		}	
		joint_info.status = gb_status_machine;
		
		save_joint_info(&joint_info);
		/*	force sensor test	*/				
		sbt_control_polling();		
		
		/*	ROS communication	*/		
		ros_send_data(&joint_info);
		
		/*	PC debug Code Begin	*/
		if(scope_count >= 10)
		{
			scope_count = 0;		
			DataScope_Get_Channel_Data(joint_info.f_act,1);
			DataScope_Get_Channel_Data(joint_info.f_des,2);
			DataScope_Get_Channel_Data(joint_info.pwm,3);
//			DataScope_Get_Channel_Data(joint_info.angle_des,4);
//			DataScope_Get_Channel_Data(joint_info.inc[CURRENT],5);
			Send_Count = DataScope_Data_Generate(3);
			HAL_UART_Transmit_DMA(&huart1,DataScope_OutPut_Buffer, Send_Count);
		}
		else
			scope_count++;
		jscope_force_raw = joint_info.f_act * 1000;
		/*	PC debug Code End		*/		
		
		/*system loop delay 1000Hz*/
    osDelay(1);
  }
}

#ifdef ROS_DEBUG
void ros_gpio_start_command(void)
{
	sys_cmd.main_mode = GB_START;
  sys_cmd.sub_mode = GB_IMPADENCE;
}

FSM_STATUS get_fsm_status(void)
{
	return gb_status_machine;
}

void status_reset(void)
{
	gb_status_machine = FSM_INITIAL;
	joint_info.error_code = 0;
	sbt_force.com_error = 0;
	sbt_force.over_time_error = 0;
	sbt_initial();
	enc_reset_create();
	pid_reset(&joint_info);
}
#endif
/* Private functions ---------------------------------------------------------*/

/****************************** END OF FILE ***********************************/
